/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * File: SMO_FOC.c
 *
 * Code generated for Simulink model 'SMO_Arctan_FOC'.
 *
 * Model version                  : 2.7
 * Simulink Coder version         : 9.8 (R2022b) 13-May-2022
 * C/C++ source code generated on : Fri Oct 11 14:25:16 2024
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: Intel->x86-64 (Windows64)
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "rtwtypes.h"
#include "SMO_FOC.h"
#include "SMO_Arctan_FOC_private.h"
#include "rt_nonfinite.h"
#include <math.h>
#include "rt_defines.h"
#include "SMO_Arctan_FOC.h"
#include <float.h>

/*
 * Output and update for action system:
 *    '<S11>/If Action Subsystem'
 *    '<S11>/If Action Subsystem3'
 */
void SMO_Arctan_FO_IfActionSubsystem(real32_T rtu_ERR, real32_T *rty_Zalpha)
{
  /* Product: '<S13>/Divide' incorporates:
   *  Constant: '<S13>/Constant'
   *  Constant: '<S13>/Constant1'
   *  Product: '<S13>/Product'
   */
  *rty_Zalpha = rtu_ERR * SMO_Kslide / SMO_MaxSMCError;
}

/*
 * Output and update for action system:
 *    '<S11>/If Action Subsystem1'
 *    '<S11>/If Action Subsystem4'
 */
void SMO_Arctan_F_IfActionSubsystem1(real32_T *rty_Zalpha)
{
  /* SignalConversion generated from: '<S14>/Zalpha' incorporates:
   *  Constant: '<S14>/Constant'
   */
  *rty_Zalpha = SMO_Kslide;
}

/*
 * Output and update for action system:
 *    '<S11>/If Action Subsystem2'
 *    '<S11>/If Action Subsystem5'
 */
void SMO_Arctan_F_IfActionSubsystem2(real32_T *rty_Zalpha)
{
  /* Gain: '<S15>/Gain' incorporates:
   *  Constant: '<S15>/Constant'
   */
  *rty_Zalpha = -SMO_Kslide;
}

/*
 * Output and update for action system:
 *    '<S32>/If Action Subsystem'
 *    '<S69>/If Action Subsystem'
 */
void SMO_Arctan__IfActionSubsystem_i(real32_T rtu_Theta, real32_T *rty_SMO_Theta)
{
  /* Sum: '<S33>/Add' incorporates:
   *  Constant: '<S33>/Constant'
   */
  *rty_SMO_Theta = rtu_Theta - 6.28318548F;
}

/*
 * Output and update for action system:
 *    '<S32>/If Action Subsystem1'
 *    '<S69>/If Action Subsystem1'
 */
void SMO_Arctan_IfActionSubsystem1_g(real32_T rtu_Theta, real32_T *rty_SMO_Theta)
{
  /* Sum: '<S34>/Add' incorporates:
   *  Constant: '<S34>/Constant'
   */
  *rty_SMO_Theta = rtu_Theta + 6.28318548F;
}

real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
{
  real32_T y;
  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
    y = (rtNaNF);
  } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
    int32_T tmp;
    int32_T tmp_0;
    if (u0 > 0.0F) {
      tmp = 1;
    } else {
      tmp = -1;
    }

    if (u1 > 0.0F) {
      tmp_0 = 1;
    } else {
      tmp_0 = -1;
    }

    y = atan2f((real32_T)tmp, (real32_T)tmp_0);
  } else if (u1 == 0.0F) {
    if (u0 > 0.0F) {
      y = RT_PIF / 2.0F;
    } else if (u0 < 0.0F) {
      y = -(RT_PIF / 2.0F);
    } else {
      y = 0.0F;
    }
  } else {
    y = atan2f(u0, u1);
  }

  return y;
}

/*
 * Output and update for atomic system:
 *    '<S12>/atan2'
 *    '<S43>/atan2'
 */
void SMO_Arctan_FOC_atan2(real32_T rtu_A, real32_T rtu_B, real32_T *rty_Y,
  uint16_T rtp_outputUnit, B_atan2_SMO_Arctan_FOC_T *localB)
{
  real32_T rtb_Atan2;

  /* Trigonometry: '<S25>/Atan2' */
  rtb_Atan2 = rt_atan2f_snf(rtu_A, rtu_B);

  /* Outputs for Enabled SubSystem: '<S25>/If Action Subsystem' incorporates:
   *  EnablePort: '<S40>/Enable'
   */
  /* RelationalOperator: '<S38>/Compare' incorporates:
   *  Constant: '<S25>/Constant'
   *  Constant: '<S38>/Constant'
   */
  if (rtp_outputUnit == 1) {
    /* Gain: '<S41>/Gain' incorporates:
     *  Merge: '<S25>/Merge'
     *  SignalConversion generated from: '<S40>/In1'
     */
    localB->Merge = rtb_Atan2;
  }

  /* End of RelationalOperator: '<S38>/Compare' */
  /* End of Outputs for SubSystem: '<S25>/If Action Subsystem' */

  /* Outputs for Enabled SubSystem: '<S25>/per Uint' incorporates:
   *  EnablePort: '<S41>/Enable'
   */
  /* RelationalOperator: '<S39>/Compare' incorporates:
   *  Constant: '<S25>/Constant'
   *  Constant: '<S39>/Constant'
   */
  if (rtp_outputUnit == 2) {
    /* Gain: '<S41>/Gain' */
    localB->Merge = 0.159154937F * rtb_Atan2;

    /* Switch: '<S41>/Switch' */
    if (!(localB->Merge >= 0.0F)) {
      /* Gain: '<S41>/Gain' incorporates:
       *  Bias: '<S41>/Bias'
       *  Merge: '<S25>/Merge'
       */
      localB->Merge++;
    }

    /* End of Switch: '<S41>/Switch' */
  }

  /* End of RelationalOperator: '<S39>/Compare' */
  /* End of Outputs for SubSystem: '<S25>/per Uint' */

  /* AlgorithmDescriptorDelegate generated from: '<S25>/a16' */
  *rty_Y = localB->Merge;
}

/*
 * Output and update for action system:
 *    '<S42>/If Action Subsystem4'
 *    '<S42>/If Action Subsystem1'
 */
void SMO_Arctan_F_IfActionSubsystem4(real32_T *rty_Zalpha)
{
  /* SignalConversion generated from: '<S48>/Zalpha' incorporates:
   *  Constant: '<S48>/Constant'
   */
  *rty_Zalpha = 0.65F;
}

/*
 * Output and update for action system:
 *    '<S42>/If Action Subsystem5'
 *    '<S42>/If Action Subsystem2'
 */
void SMO_Arctan_F_IfActionSubsystem5(real32_T *rty_Zalpha)
{
  /* Gain: '<S49>/Gain' incorporates:
   *  Constant: '<S49>/Constant'
   */
  *rty_Zalpha = -0.65F;
}

real32_T rt_modf_snf(real32_T u0, real32_T u1)
{
  real32_T y;
  y = u0;
  if (u1 == 0.0F) {
    if (u0 == 0.0F) {
      y = u1;
    }
  } else if (rtIsNaNF(u0) || rtIsNaNF(u1) || rtIsInfF(u0)) {
    y = (rtNaNF);
  } else if (u0 == 0.0F) {
    y = 0.0F / u1;
  } else if (rtIsInfF(u1)) {
    if ((u1 < 0.0F) != (u0 < 0.0F)) {
      y = u1;
    }
  } else {
    boolean_T yEq;
    y = fmodf(u0, u1);
    yEq = (y == 0.0F);
    if ((!yEq) && (u1 > floorf(u1))) {
      real32_T q;
      q = fabsf(u0 / u1);
      yEq = !(fabsf(q - floorf(q + 0.5F)) > FLT_EPSILON * q);
    }

    if (yEq) {
      y = u1 * 0.0F;
    } else if ((u0 < 0.0F) != (u1 < 0.0F)) {
      y += u1;
    }
  }

  return y;
}

/* System initialize for function-call system: '<S1>/SMO_FOC' */
void SMO_Arctan_FOC_SMO_FOC_Init(void)
{
  /* Start for SwitchCase: '<S8>/Switch Case' */
  SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem = -1;
}

/* Enable for function-call system: '<S1>/SMO_FOC' */
void SMO_Arctan_FOC_SMO_FOC_Enable(void)
{
  SMO_Arctan_FOC_DW.SMO_FOC_RESET_ELAPS_T = true;

  /* Enable for DiscreteIntegrator: '<S11>/Discrete-Time Integrator' */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_E = 1U;

  /* Enable for DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTEM_ = 1U;

  /* Enable for DiscreteIntegrator: '<S217>/Integrator' */
  SMO_Arctan_FOC_DW.Integrator_SYSTEM_ENABLE = 1U;

  /* Enable for DiscreteIntegrator: '<S265>/Integrator' */
  SMO_Arctan_FOC_DW.Integrator_SYSTEM_ENABLE_j = 1U;

  /* Enable for DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_c = 1U;

  /* Enable for DiscreteIntegrator: '<S42>/Discrete-Time Integrator1' */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTE_g = 1U;
}

/* Output and update for function-call system: '<S1>/SMO_FOC' */
void SMO_Arctan_FOC_SMO_FOC(void)
{
  real_T tmp;
  int32_T sector;
  real32_T T1;
  real32_T T2;
  real32_T beta;
  real32_T rtb_DTC;
  real32_T rtb_ERR;
  real32_T rtb_ERR_d;
  real32_T rtb_Merge;
  real32_T rtb_Merge1;
  real32_T rtb_Merge_c;
  real32_T rtb_Saturation;
  real32_T rtb_Tcmp2;
  real32_T rtb_Tcmp3;
  real32_T rtb_ialpha;
  real32_T rtb_ibeta;
  real32_T ta;
  uint32_T SMO_FOC_ELAPS_T;
  uint32_T SwitchCaseActionSubsystem1_ELAP;
  int8_T rtPrevAction;
  if (SMO_Arctan_FOC_DW.SMO_FOC_RESET_ELAPS_T) {
    SMO_FOC_ELAPS_T = 0U;
  } else {
    SMO_FOC_ELAPS_T = SMO_Arctan_FOC_M->Timing.clockTick0 -
      SMO_Arctan_FOC_DW.SMO_FOC_PREV_T;
  }

  /* Update for IfAction SubSystem: '<S8>/Switch Case Action Subsystem1' incorporates:
   *  ActionPort: '<S296>/Action Port'
   */
  /* Outputs for IfAction SubSystem: '<S8>/Switch Case Action Subsystem1' incorporates:
   *  ActionPort: '<S296>/Action Port'
   */
  /* SwitchCase: '<S8>/Switch Case' */
  SMO_Arctan_FOC_DW.SMO_FOC_PREV_T = SMO_Arctan_FOC_M->Timing.clockTick0;

  /* End of Outputs for SubSystem: '<S8>/Switch Case Action Subsystem1' */
  /* End of Update for SubSystem: '<S8>/Switch Case Action Subsystem1' */
  SMO_Arctan_FOC_DW.SMO_FOC_RESET_ELAPS_T = false;

  /* Saturate: '<S9>/Saturation' incorporates:
   *  Delay: '<S9>/Delay'
   */
  if (SMO_Arctan_FOC_DW.Delay_DSTATE_a > 5000.0F) {
    rtb_Merge1 = 5000.0F;
  } else if (SMO_Arctan_FOC_DW.Delay_DSTATE_a < 10.0F) {
    rtb_Merge1 = 10.0F;
  } else {
    rtb_Merge1 = SMO_Arctan_FOC_DW.Delay_DSTATE_a;
  }

  /* End of Saturate: '<S9>/Saturation' */

  /* DiscreteIntegrator: '<S11>/Discrete-Time Integrator' */
  if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_E == 0) {
    /* DiscreteIntegrator: '<S11>/Discrete-Time Integrator' */
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE += 5.0E-5F * (real32_T)
      SMO_FOC_ELAPS_T * SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_PREV_U;
  }

  /* End of DiscreteIntegrator: '<S11>/Discrete-Time Integrator' */

  /* MATLAB Function: '<S6>/Clark2' incorporates:
   *  Inport: '<Root>/Iabc'
   */
  rtb_ialpha = SMO_Arctan_FOC_U.iabc[0] * 2.0F / 3.0F - (SMO_Arctan_FOC_U.iabc[1]
    + SMO_Arctan_FOC_U.iabc[2]) / 3.0F;
  rtb_ibeta = (SMO_Arctan_FOC_U.iabc[1] - SMO_Arctan_FOC_U.iabc[2]) *
    1.73205078F / 3.0F;

  /* Sum: '<S11>/Sum' */
  rtb_ERR = SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE - rtb_ialpha;

  /* If: '<S11>/If' */
  if (rtb_ERR < 1.5F) {
    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem' incorporates:
     *  ActionPort: '<S13>/Action Port'
     */
    SMO_Arctan_FO_IfActionSubsystem(rtb_ERR, &rtb_Merge_c);

    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem' */
  } else if (rtb_ERR > 0.0F) {
    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S14>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem1(&rtb_Merge_c);

    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem1' */
  } else {
    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem2' incorporates:
     *  ActionPort: '<S15>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem2(&rtb_Merge_c);

    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem2' */
  }

  /* End of If: '<S11>/If' */

  /* Sum: '<S19>/Add' incorporates:
   *  Delay: '<S19>/Delay'
   *  Gain: '<S19>/Gain'
   *  Product: '<S19>/Product'
   *  Sum: '<S19>/Add1'
   */
  SMO_Arctan_FOC_DW.Delay_DSTATE_c += 5.0E-5F * rtb_Merge1 * (rtb_Merge_c -
    SMO_Arctan_FOC_DW.Delay_DSTATE_c);

  /* Gain: '<S20>/Gain' */
  rtb_ERR = 5.0E-5F * rtb_Merge1;

  /* DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */
  if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTEM_ == 0) {
    /* DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTATE += 5.0E-5F * (real32_T)
      SMO_FOC_ELAPS_T * SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_PREV_U;
  }

  /* End of DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */

  /* Sum: '<S11>/Sum1' */
  rtb_ERR_d = SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTATE - rtb_ibeta;

  /* If: '<S11>/If1' */
  if (rtb_ERR_d < 1.5F) {
    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem3' incorporates:
     *  ActionPort: '<S16>/Action Port'
     */
    SMO_Arctan_FO_IfActionSubsystem(rtb_ERR_d, &rtb_Merge1);

    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem3' */
  } else if (rtb_ERR_d > 0.0F) {
    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem4' incorporates:
     *  ActionPort: '<S17>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem1(&rtb_Merge1);

    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem4' */
  } else {
    /* Outputs for IfAction SubSystem: '<S11>/If Action Subsystem5' incorporates:
     *  ActionPort: '<S18>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem2(&rtb_Merge1);

    /* End of Outputs for SubSystem: '<S11>/If Action Subsystem5' */
  }

  /* End of If: '<S11>/If1' */

  /* Sum: '<S20>/Add' incorporates:
   *  Delay: '<S20>/Delay'
   *  Product: '<S20>/Product'
   *  Sum: '<S20>/Add1'
   */
  SMO_Arctan_FOC_DW.Delay_DSTATE_i += (rtb_Merge1 -
    SMO_Arctan_FOC_DW.Delay_DSTATE_i) * rtb_ERR;

  /* Outputs for Atomic SubSystem: '<S12>/atan2' */
  SMO_Arctan_FOC_atan2(SMO_Arctan_FOC_DW.Delay_DSTATE_c,
                       SMO_Arctan_FOC_DW.Delay_DSTATE_i, &rtb_DTC, 1,
                       &SMO_Arctan_FOC_B.atan2_i);

  /* End of Outputs for SubSystem: '<S12>/atan2' */

  /* Sum: '<S23>/Add' incorporates:
   *  Constant: '<S12>/Constant'
   *  Constant: '<S23>/Constant1'
   *  Gain: '<S12>/Gain'
   *  Math: '<S12>/Math Function'
   */
  rtb_ERR = rt_modf_snf(-rtb_DTC, 6.28318548F) + SMO_OffsetAngle;

  /* If: '<S32>/If' */
  if (rtb_ERR > 6.2832F) {
    /* Outputs for IfAction SubSystem: '<S32>/If Action Subsystem' incorporates:
     *  ActionPort: '<S33>/Action Port'
     */
    SMO_Arctan__IfActionSubsystem_i(rtb_ERR, &rtb_Merge);

    /* End of Outputs for SubSystem: '<S32>/If Action Subsystem' */
  } else if (rtb_ERR < 0.0F) {
    /* Outputs for IfAction SubSystem: '<S32>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S34>/Action Port'
     */
    SMO_Arctan_IfActionSubsystem1_g(rtb_ERR, &rtb_Merge);

    /* End of Outputs for SubSystem: '<S32>/If Action Subsystem1' */
  } else {
    /* Outputs for IfAction SubSystem: '<S32>/If Action Subsystem2' incorporates:
     *  ActionPort: '<S35>/Action Port'
     */
    /* SignalConversion generated from: '<S35>/Theta' */
    rtb_Merge = rtb_ERR;

    /* End of Outputs for SubSystem: '<S32>/If Action Subsystem2' */
  }

  /* End of If: '<S32>/If' */

  /* SwitchCase: '<S8>/Switch Case' */
  rtPrevAction = SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem;
  SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem = -1;
  tmp = trunc(SMO_Arctan_FOC_B.Motor_State);
  if (rtIsNaN(tmp) || rtIsInf(tmp)) {
    tmp = 0.0;
  } else {
    tmp = fmod(tmp, 4.294967296E+9);
  }

  switch (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)(uint32_T)tmp) {
   case 0:
    SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem = 0;
    break;

   case 1:
    SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem = 1;
    break;

   case 2:
    SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem = 2;
    break;
  }

  switch (SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem) {
   case 0:
    /* Outputs for IfAction SubSystem: '<S8>/Switch Case Action Subsystem' incorporates:
     *  ActionPort: '<S295>/Action Port'
     */
    /* Merge: '<S8>/Merge' incorporates:
     *  Constant: '<S295>/Constant1'
     *  SignalConversion generated from: '<S295>/Id_Ref'
     */
    SMO_Arctan_FOC_B.Merge = 1.0F;

    /* Merge: '<S8>/Merge1' incorporates:
     *  Constant: '<S295>/Constant'
     *  SignalConversion generated from: '<S295>/Iq_Ref'
     */
    SMO_Arctan_FOC_B.Merge1 = 0.0F;

    /* Merge: '<S8>/Merge2' incorporates:
     *  Constant: '<S295>/Constant2'
     *  SignalConversion generated from: '<S295>/Theta'
     */
    SMO_Arctan_FOC_B.Merge2 = 0.0F;

    /* End of Outputs for SubSystem: '<S8>/Switch Case Action Subsystem' */
    break;

   case 1:
    if (SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem != rtPrevAction) {
      SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_RESE = true;

      /* Enable for IfAction SubSystem: '<S8>/Switch Case Action Subsystem1' incorporates:
       *  ActionPort: '<S296>/Action Port'
       */
      /* Enable for SwitchCase: '<S8>/Switch Case' incorporates:
       *  DiscreteIntegrator: '<S296>/Discrete-Time Integrator'
       *  DiscreteIntegrator: '<S296>/Discrete-Time Integrator1'
       */
      SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_n = 1U;
      SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTE_m = 1U;

      /* End of Enable for SubSystem: '<S8>/Switch Case Action Subsystem1' */
    }

    /* Outputs for IfAction SubSystem: '<S8>/Switch Case Action Subsystem1' incorporates:
     *  ActionPort: '<S296>/Action Port'
     */
    if (SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_RESE) {
      SwitchCaseActionSubsystem1_ELAP = 0U;
    } else {
      SwitchCaseActionSubsystem1_ELAP = SMO_Arctan_FOC_DW.SMO_FOC_PREV_T
        - SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_PREV;
    }

    SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_PREV =
      SMO_Arctan_FOC_DW.SMO_FOC_PREV_T;
    SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_RESE = false;

    /* Merge: '<S8>/Merge' incorporates:
     *  Constant: '<S296>/Constant1'
     *  SignalConversion generated from: '<S296>/Id_Ref'
     */
    SMO_Arctan_FOC_B.Merge = 1.0F;

    /* Merge: '<S8>/Merge1' incorporates:
     *  Constant: '<S296>/Constant'
     *  SignalConversion generated from: '<S296>/Iq_Ref'
     */
    SMO_Arctan_FOC_B.Merge1 = 0.0F;

    /* DiscreteIntegrator: '<S296>/Discrete-Time Integrator' */
    if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_n == 0) {
      /* DiscreteIntegrator: '<S296>/Discrete-Time Integrator' */
      SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE_e += 5.0E-5F * (real32_T)
        SwitchCaseActionSubsystem1_ELAP *
        SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_PREV_U_b;
    }

    /* End of DiscreteIntegrator: '<S296>/Discrete-Time Integrator' */

    /* DiscreteIntegrator: '<S296>/Discrete-Time Integrator1' */
    if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTE_m == 0) {
      /* DiscreteIntegrator: '<S296>/Discrete-Time Integrator1' */
      SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTAT_l += 5.0E-5F * (real32_T)
        SwitchCaseActionSubsystem1_ELAP *
        SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_PREV__o;
    }

    /* End of DiscreteIntegrator: '<S296>/Discrete-Time Integrator1' */

    /* Merge: '<S8>/Merge2' incorporates:
     *  Constant: '<S296>/Constant4'
     *  Math: '<S296>/Math Function'
     *  SignalConversion generated from: '<S296>/Theta'
     */
    SMO_Arctan_FOC_B.Merge2 = rt_modf_snf
      (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTAT_l, 6.28318548F);

    /* Update for DiscreteIntegrator: '<S296>/Discrete-Time Integrator' */
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_n = 0U;
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_PREV_U_b =
      SMO_Arctan_FOC_ConstB.Divide;

    /* Update for DiscreteIntegrator: '<S296>/Discrete-Time Integrator1' incorporates:
     *  Gain: '<S296>/Gain'
     *  Gain: '<S296>/Gain1'
     */
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTE_m = 0U;
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_PREV__o = 0.116666667F *
      SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE_e * 6.28318548F;

    /* End of Outputs for SubSystem: '<S8>/Switch Case Action Subsystem1' */
    break;

   case 2:
    /* Outputs for IfAction SubSystem: '<S8>/Switch Case Action Subsystem2' incorporates:
     *  ActionPort: '<S297>/Action Port'
     */
    /* Merge: '<S8>/Merge' incorporates:
     *  SignalConversion generated from: '<S297>/Id'
     */
    SMO_Arctan_FOC_B.Merge = SMO_Arctan_FOC_B.OutportBufferForId_Ref;

    /* Merge: '<S8>/Merge1' incorporates:
     *  SignalConversion generated from: '<S297>/Iq'
     */
    SMO_Arctan_FOC_B.Merge1 = SMO_Arctan_FOC_B.Saturation;

    /* Merge: '<S8>/Merge2' incorporates:
     *  SignalConversion generated from: '<S297>/SMO_Theta'
     */
    SMO_Arctan_FOC_B.Merge2 = rtb_Merge;

    /* End of Outputs for SubSystem: '<S8>/Switch Case Action Subsystem2' */
    break;
  }

  /* Sum: '<S6>/Sum2' incorporates:
   *  MATLAB Function: '<S6>/Park2'
   */
  rtb_ERR = SMO_Arctan_FOC_B.Merge - (rtb_ialpha * cosf(SMO_Arctan_FOC_B.Merge2)
    + rtb_ibeta * sinf(SMO_Arctan_FOC_B.Merge2));

  /* DiscreteIntegrator: '<S217>/Integrator' */
  if (SMO_Arctan_FOC_DW.Integrator_SYSTEM_ENABLE == 0) {
    /* DiscreteIntegrator: '<S217>/Integrator' */
    SMO_Arctan_FOC_DW.Integrator_DSTATE += 5.0E-5F * (real32_T)SMO_FOC_ELAPS_T *
      SMO_Arctan_FOC_DW.Integrator_PREV_U;
  }

  /* End of DiscreteIntegrator: '<S217>/Integrator' */

  /* Sum: '<S226>/Sum' incorporates:
   *  Gain: '<S222>/Proportional Gain'
   */
  rtb_Saturation = 0.194F * rtb_ERR + SMO_Arctan_FOC_DW.Integrator_DSTATE;

  /* Saturate: '<S224>/Saturation' */
  if (rtb_Saturation > 13.8564062F) {
    rtb_Saturation = 13.8564062F;
  } else if (rtb_Saturation < -13.8564062F) {
    rtb_Saturation = -13.8564062F;
  }

  /* End of Saturate: '<S224>/Saturation' */

  /* Sum: '<S6>/Sum3' incorporates:
   *  MATLAB Function: '<S6>/Park2'
   */
  rtb_ERR_d = SMO_Arctan_FOC_B.Merge1 - (-rtb_ialpha * sinf
    (SMO_Arctan_FOC_B.Merge2) + rtb_ibeta * cosf(SMO_Arctan_FOC_B.Merge2));

  /* DiscreteIntegrator: '<S265>/Integrator' */
  if (SMO_Arctan_FOC_DW.Integrator_SYSTEM_ENABLE_j == 0) {
    /* DiscreteIntegrator: '<S265>/Integrator' */
    SMO_Arctan_FOC_DW.Integrator_DSTATE_p += 5.0E-5F * (real32_T)SMO_FOC_ELAPS_T
      * SMO_Arctan_FOC_DW.Integrator_PREV_U_i;
  }

  /* End of DiscreteIntegrator: '<S265>/Integrator' */

  /* Sum: '<S274>/Sum' incorporates:
   *  Gain: '<S270>/Proportional Gain'
   */
  rtb_DTC = 0.194F * rtb_ERR_d + SMO_Arctan_FOC_DW.Integrator_DSTATE_p;

  /* Saturate: '<S272>/Saturation' */
  if (rtb_DTC > 13.8564062F) {
    rtb_DTC = 13.8564062F;
  } else if (rtb_DTC < -13.8564062F) {
    rtb_DTC = -13.8564062F;
  }

  /* End of Saturate: '<S272>/Saturation' */

  /* MATLAB Function: '<S6>/foc1' incorporates:
   *  Constant: '<S6>/Constant'
   *  Constant: '<S6>/Constant1'
   */
  beta = atanf(rtb_DTC / fabsf(rtb_Saturation));
  if (rtb_Saturation * rtb_Saturation + rtb_DTC * rtb_DTC > 192.0F) {
    rtb_Tcmp2 = cosf(beta);
    rtb_Saturation = -13.8564062F * rtb_Tcmp2;
    rtb_DTC = 13.8564062F * sinf(beta);
    if (rtb_Saturation > 0.0F) {
      rtb_Saturation = 13.8564062F * rtb_Tcmp2;
    }
  }

  rtb_Tcmp2 = sinf(SMO_Arctan_FOC_B.Merge2);
  rtb_Tcmp3 = cosf(SMO_Arctan_FOC_B.Merge2);
  beta = rtb_Saturation * rtb_Tcmp3 - rtb_DTC * rtb_Tcmp2;
  rtb_Saturation = rtb_Saturation * rtb_Tcmp2 + rtb_DTC * rtb_Tcmp3;
  sector = 0;
  rtb_DTC = 0.0F;
  rtb_Tcmp2 = 0.0F;
  rtb_Tcmp3 = 0.0F;
  if (rtb_Saturation > 0.0F) {
    sector = 1;
  }

  if ((1.73205078F * beta - rtb_Saturation) / 2.0F > 0.0F) {
    sector += 2;
  }

  if ((-1.73205078F * beta - rtb_Saturation) / 2.0F > 0.0F) {
    sector += 4;
  }

  switch (sector) {
   case 1:
    T1 = (-1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F;
    T2 = (1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F;
    break;

   case 2:
    T1 = (1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F;
    T2 = -(1.73205078F * rtb_Saturation * 8192.0F / 24.0F);
    break;

   case 3:
    T1 = -((-1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F);
    T2 = 1.73205078F * rtb_Saturation * 8192.0F / 24.0F;
    break;

   case 4:
    T1 = -(1.73205078F * rtb_Saturation * 8192.0F / 24.0F);
    T2 = (-1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F;
    break;

   case 5:
    T1 = 1.73205078F * rtb_Saturation * 8192.0F / 24.0F;
    T2 = -((1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F);
    break;

   default:
    T1 = -((1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F);
    T2 = -((-1.5F * beta + 0.866025388F * rtb_Saturation) * 341.333344F);
    break;
  }

  ta = (8192.0F - (T1 + T2)) / 4.0F;
  T1 = T1 / 2.0F + ta;
  T2 = T2 / 2.0F + T1;
  ta = roundf(ta);
  T1 = roundf(T1);
  switch (sector) {
   case 1:
    rtb_DTC = T1;
    rtb_Tcmp2 = ta;
    rtb_Tcmp3 = roundf(T2);
    break;

   case 2:
    rtb_DTC = ta;
    rtb_Tcmp2 = roundf(T2);
    rtb_Tcmp3 = T1;
    break;

   case 3:
    rtb_DTC = ta;
    rtb_Tcmp2 = T1;
    rtb_Tcmp3 = roundf(T2);
    break;

   case 4:
    rtb_DTC = roundf(T2);
    rtb_Tcmp2 = T1;
    rtb_Tcmp3 = ta;
    break;

   case 5:
    rtb_DTC = roundf(T2);
    rtb_Tcmp2 = ta;
    rtb_Tcmp3 = T1;
    break;

   case 6:
    rtb_DTC = T1;
    rtb_Tcmp2 = roundf(T2);
    rtb_Tcmp3 = ta;
    break;
  }

  /* Sum: '<S6>/Sum6' incorporates:
   *  Constant: '<S6>/Constant3'
   *  Constant: '<S6>/Constant4'
   *  Gain: '<S6>/Gain'
   *  Gain: '<S6>/Gain1'
   *  Sum: '<S6>/Sum4'
   *  Sum: '<S6>/Sum5'
   */
  SMO_Arctan_FOC_B.Sum6[0] = -(0.000244140625F * rtb_DTC - 0.5F) + 0.5F;
  SMO_Arctan_FOC_B.Sum6[1] = -(0.000244140625F * rtb_Tcmp2 - 0.5F) + 0.5F;
  SMO_Arctan_FOC_B.Sum6[2] = -(0.000244140625F * rtb_Tcmp3 - 0.5F) + 0.5F;

  /* Gain: '<S11>/Gain1' incorporates:
   *  MATLAB Function: '<S6>/foc1'
   *  Sum: '<S11>/Add1'
   */
  rtb_Tcmp2 = ((beta - SMO_Arctan_FOC_DW.Delay_DSTATE_c) - rtb_Merge_c) *
    10309.2783F;

  /* DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
  if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_c == 0) {
    /* DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE_o += 5.0E-5F * (real32_T)
      SMO_FOC_ELAPS_T * SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_PREV_U_l;
  }

  /* End of DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */

  /* If: '<S42>/If2' incorporates:
   *  Sum: '<S42>/Sum'
   */
  if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE_o - rtb_ialpha > 0.0F) {
    /* Outputs for IfAction SubSystem: '<S42>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S46>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem4(&rtb_DTC);

    /* End of Outputs for SubSystem: '<S42>/If Action Subsystem1' */
  } else {
    /* Outputs for IfAction SubSystem: '<S42>/If Action Subsystem2' incorporates:
     *  ActionPort: '<S47>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem5(&rtb_DTC);

    /* End of Outputs for SubSystem: '<S42>/If Action Subsystem2' */
  }

  /* End of If: '<S42>/If2' */

  /* Gain: '<S42>/Gain1' incorporates:
   *  MATLAB Function: '<S6>/foc1'
   *  Sum: '<S42>/Add1'
   */
  rtb_ialpha = (beta - rtb_DTC) * 10309.2783F;

  /* Sum: '<S50>/Add1' incorporates:
   *  Delay: '<S50>/Delay'
   */
  rtb_Merge_c = rtb_DTC - SMO_Arctan_FOC_DW.Delay_DSTATE_g;

  /* Saturate: '<S10>/Saturation' incorporates:
   *  Delay: '<S10>/Delay'
   */
  if (SMO_Arctan_FOC_DW.Delay_DSTATE_gm > 2000.0F) {
    rtb_DTC = 2000.0F;
  } else if (SMO_Arctan_FOC_DW.Delay_DSTATE_gm < 10.0F) {
    rtb_DTC = 10.0F;
  } else {
    rtb_DTC = SMO_Arctan_FOC_DW.Delay_DSTATE_gm;
  }

  /* End of Saturate: '<S10>/Saturation' */

  /* Sum: '<S50>/Add' incorporates:
   *  Delay: '<S50>/Delay'
   *  Gain: '<S50>/Gain'
   *  Product: '<S50>/Product'
   */
  SMO_Arctan_FOC_DW.Delay_DSTATE_g += 5.0E-5F * rtb_DTC * rtb_Merge_c;

  /* Sum: '<S54>/Add1' incorporates:
   *  Constant: '<S54>/Filter_Constant'
   *  Constant: '<S54>/One'
   *  Product: '<S54>/Product'
   *  Product: '<S54>/Product1'
   *  UnitDelay: '<S54>/Unit Delay'
   */
  SMO_Arctan_FOC_DW.UnitDelay_DSTATE = SMO_Arctan_FOC_DW.Delay_DSTATE_g * 0.15F
    + 0.85F * SMO_Arctan_FOC_DW.UnitDelay_DSTATE;

  /* Gain: '<S51>/Gain' */
  rtb_Merge_c = 5.0E-5F * rtb_DTC;

  /* DiscreteIntegrator: '<S42>/Discrete-Time Integrator1' */
  if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTE_g == 0) {
    /* DiscreteIntegrator: '<S42>/Discrete-Time Integrator1' */
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTAT_d += 5.0E-5F * (real32_T)
      SMO_FOC_ELAPS_T * SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_PREV__g;
  }

  /* End of DiscreteIntegrator: '<S42>/Discrete-Time Integrator1' */

  /* If: '<S42>/If1' incorporates:
   *  Sum: '<S42>/Sum1'
   */
  if (SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTAT_d - rtb_ibeta > 0.0F) {
    /* Outputs for IfAction SubSystem: '<S42>/If Action Subsystem4' incorporates:
     *  ActionPort: '<S48>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem4(&rtb_DTC);

    /* End of Outputs for SubSystem: '<S42>/If Action Subsystem4' */
  } else {
    /* Outputs for IfAction SubSystem: '<S42>/If Action Subsystem5' incorporates:
     *  ActionPort: '<S49>/Action Port'
     */
    SMO_Arctan_F_IfActionSubsystem5(&rtb_DTC);

    /* End of Outputs for SubSystem: '<S42>/If Action Subsystem5' */
  }

  /* End of If: '<S42>/If1' */

  /* Sum: '<S51>/Add' incorporates:
   *  Delay: '<S51>/Delay'
   *  Product: '<S51>/Product'
   *  Sum: '<S51>/Add1'
   */
  SMO_Arctan_FOC_DW.Delay_DSTATE_k += (rtb_DTC -
    SMO_Arctan_FOC_DW.Delay_DSTATE_k) * rtb_Merge_c;

  /* Sum: '<S57>/Add1' incorporates:
   *  Constant: '<S57>/Filter_Constant'
   *  Constant: '<S57>/One'
   *  Product: '<S57>/Product'
   *  Product: '<S57>/Product1'
   *  UnitDelay: '<S57>/Unit Delay'
   */
  SMO_Arctan_FOC_DW.UnitDelay_DSTATE_n = SMO_Arctan_FOC_DW.Delay_DSTATE_k *
    0.15F + 0.85F * SMO_Arctan_FOC_DW.UnitDelay_DSTATE_n;

  /* Outputs for Atomic SubSystem: '<S43>/atan2' */
  SMO_Arctan_FOC_atan2(SMO_Arctan_FOC_DW.UnitDelay_DSTATE,
                       SMO_Arctan_FOC_DW.UnitDelay_DSTATE_n, &rtb_Merge_c, 1,
                       &SMO_Arctan_FOC_B.atan2_b);

  /* End of Outputs for SubSystem: '<S43>/atan2' */

  /* Math: '<S43>/Math Function' incorporates:
   *  Constant: '<S43>/Constant'
   *  Gain: '<S43>/Gain'
   */
  rtb_Merge_c = rt_modf_snf(-rtb_Merge_c, 6.28318548F);

  /* If: '<S69>/If' incorporates:
   *  Constant: '<S60>/Constant1'
   *  Sum: '<S60>/Add'
   */
  if (rtb_Merge_c + 0.785398185F > 6.2832F) {
    /* Outputs for IfAction SubSystem: '<S69>/If Action Subsystem' incorporates:
     *  ActionPort: '<S70>/Action Port'
     */
    SMO_Arctan__IfActionSubsystem_i(rtb_Merge_c + 0.785398185F, &rtb_Merge_c);

    /* End of Outputs for SubSystem: '<S69>/If Action Subsystem' */
  } else {
    /* Outputs for IfAction SubSystem: '<S69>/If Action Subsystem2' incorporates:
     *  ActionPort: '<S72>/Action Port'
     */
    /* SignalConversion generated from: '<S72>/Theta' */
    rtb_Merge_c += 0.785398185F;

    /* End of Outputs for SubSystem: '<S69>/If Action Subsystem2' */
  }

  /* End of If: '<S69>/If' */

  /* Gain: '<S24>/PositionToCount' */
  rtb_ibeta = truncf(6.83563648E+8F * rtb_Merge);
  if (rtIsNaNF(rtb_ibeta) || rtIsInfF(rtb_ibeta)) {
    rtb_ibeta = 0.0F;
  } else {
    rtb_ibeta = fmodf(rtb_ibeta, 4.2949673E+9F);
  }

  SMO_FOC_ELAPS_T = rtb_ibeta < 0.0F ? (uint32_T)-(int32_T)(uint32_T)-rtb_ibeta :
    (uint32_T)rtb_ibeta;

  /* End of Gain: '<S24>/PositionToCount' */

  /* Sum: '<S28>/Add1' incorporates:
   *  Constant: '<S28>/Filter_Constant'
   *  Constant: '<S28>/One'
   *  DataTypeConversion: '<S37>/DTC'
   *  Delay: '<S24>/Delay'
   *  Gain: '<S12>/Gain1'
   *  Gain: '<S24>/SpeedGain'
   *  Product: '<S28>/Product'
   *  Product: '<S28>/Product1'
   *  Sum: '<S24>/SpeedCount'
   *  UnitDelay: '<S28>/Unit Delay'
   */
  SMO_Arctan_FOC_DW.UnitDelay_DSTATE_no = (real32_T)((int32_T)SMO_FOC_ELAPS_T
    - (int32_T)SMO_Arctan_FOC_DW.Delay_DSTATE_n[SMO_Arctan_FOC_DW.CircBufIdx]) *
    9.31322575E-5F * 0.142857149F * 0.02F + 0.98F *
    SMO_Arctan_FOC_DW.UnitDelay_DSTATE_no;

  /* Sum: '<S31>/Add1' incorporates:
   *  Constant: '<S31>/Filter_Constant'
   *  Constant: '<S31>/One'
   *  Gain: '<S12>/f_2_we'
   *  Gain: '<S12>/n_2_f'
   *  Product: '<S31>/Product'
   *  Product: '<S31>/Product1'
   *  UnitDelay: '<S28>/Unit Delay'
   *  UnitDelay: '<S31>/Unit Delay'
   */
  SMO_Arctan_FOC_DW.Delay_DSTATE_a = 0.116666667F *
    SMO_Arctan_FOC_DW.UnitDelay_DSTATE_no * 6.28318548F * 0.02F + 0.98F *
    SMO_Arctan_FOC_DW.UnitDelay_DSTATE_c;

  /* Gain: '<S61>/PositionToCount' */
  rtb_ibeta = truncf(6.83563648E+8F * rtb_Merge_c);
  if (rtIsNaNF(rtb_ibeta) || rtIsInfF(rtb_ibeta)) {
    rtb_ibeta = 0.0F;
  } else {
    rtb_ibeta = fmodf(rtb_ibeta, 4.2949673E+9F);
  }

  SwitchCaseActionSubsystem1_ELAP = rtb_ibeta < 0.0F ? (uint32_T)-(int32_T)
    (uint32_T)-rtb_ibeta : (uint32_T)rtb_ibeta;

  /* End of Gain: '<S61>/PositionToCount' */

  /* Sum: '<S65>/Add1' incorporates:
   *  Constant: '<S65>/Filter_Constant'
   *  Constant: '<S65>/One'
   *  DataTypeConversion: '<S74>/DTC'
   *  Delay: '<S61>/Delay'
   *  Gain: '<S43>/Gain1'
   *  Gain: '<S61>/SpeedGain'
   *  Product: '<S65>/Product'
   *  Product: '<S65>/Product1'
   *  Sum: '<S61>/SpeedCount'
   *  UnitDelay: '<S65>/Unit Delay'
   */
  SMO_Arctan_FOC_DW.UnitDelay_DSTATE_m = (real32_T)((int32_T)
    SwitchCaseActionSubsystem1_ELAP - (int32_T)
    SMO_Arctan_FOC_DW.Delay_DSTATE_ca[SMO_Arctan_FOC_DW.CircBufIdx_n]) *
    9.31322575E-5F * 0.142857149F * 0.001F + 0.999F *
    SMO_Arctan_FOC_DW.UnitDelay_DSTATE_m;

  /* Sum: '<S68>/Add1' incorporates:
   *  Constant: '<S68>/Filter_Constant'
   *  Constant: '<S68>/One'
   *  Gain: '<S43>/f_2_we'
   *  Gain: '<S43>/n_2_f'
   *  Product: '<S68>/Product'
   *  Product: '<S68>/Product1'
   *  UnitDelay: '<S68>/Unit Delay'
   */
  SMO_Arctan_FOC_DW.Delay_DSTATE_gm = 0.116666667F *
    SMO_Arctan_FOC_DW.UnitDelay_DSTATE_m * 6.28318548F * 0.001F + 0.999F *
    SMO_Arctan_FOC_DW.UnitDelay_DSTATE_l;

  /* Update for DiscreteIntegrator: '<S11>/Discrete-Time Integrator' incorporates:
   *  Gain: '<S11>/Gain'
   *  Sum: '<S11>/Add'
   */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_E = 0U;
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_PREV_U = -2000.0F *
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE + rtb_Tcmp2;

  /* Update for DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' incorporates:
   *  Gain: '<S11>/Gain2'
   *  Gain: '<S11>/Gain3'
   *  MATLAB Function: '<S6>/foc1'
   *  Sum: '<S11>/Add2'
   *  Sum: '<S11>/Add3'
   */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTEM_ = 0U;
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_PREV_U = ((rtb_Saturation -
    SMO_Arctan_FOC_DW.Delay_DSTATE_i) - rtb_Merge1) * 10309.2783F + -2000.0F *
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTATE;

  /* Update for SwitchCase: '<S8>/Switch Case' */
  if (SMO_Arctan_FOC_DW.SwitchCase_ActiveSubsystem == 1) {
    /* Update for IfAction SubSystem: '<S8>/Switch Case Action Subsystem1' incorporates:
     *  ActionPort: '<S296>/Action Port'
     */
    SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_PREV =
      SMO_Arctan_FOC_DW.SMO_FOC_PREV_T;
    SMO_Arctan_FOC_DW.SwitchCaseActionSubsystem1_RESE = false;

    /* End of Update for SubSystem: '<S8>/Switch Case Action Subsystem1' */
  }

  /* Update for DiscreteIntegrator: '<S217>/Integrator' incorporates:
   *  Gain: '<S214>/Integral Gain'
   */
  SMO_Arctan_FOC_DW.Integrator_SYSTEM_ENABLE = 0U;
  SMO_Arctan_FOC_DW.Integrator_PREV_U = 388.0F * rtb_ERR;

  /* Update for DiscreteIntegrator: '<S265>/Integrator' incorporates:
   *  Gain: '<S262>/Integral Gain'
   */
  SMO_Arctan_FOC_DW.Integrator_SYSTEM_ENABLE_j = 0U;
  SMO_Arctan_FOC_DW.Integrator_PREV_U_i = 388.0F * rtb_ERR_d;

  /* Update for DiscreteIntegrator: '<S42>/Discrete-Time Integrator' incorporates:
   *  Gain: '<S42>/Gain'
   *  Sum: '<S42>/Add'
   */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_SYSTEM_c = 0U;
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_PREV_U_l = -2000.0F *
    SMO_Arctan_FOC_DW.DiscreteTimeIntegrator_DSTATE_o + rtb_ialpha;

  /* Update for DiscreteIntegrator: '<S42>/Discrete-Time Integrator1' incorporates:
   *  Gain: '<S42>/Gain2'
   *  Gain: '<S42>/Gain3'
   *  MATLAB Function: '<S6>/foc1'
   *  Sum: '<S42>/Add2'
   *  Sum: '<S42>/Add3'
   */
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_SYSTE_g = 0U;
  SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_PREV__g = (rtb_Saturation - rtb_DTC)
    * 10309.2783F + -2000.0F * SMO_Arctan_FOC_DW.DiscreteTimeIntegrator1_DSTAT_d;

  /* Update for Delay: '<S24>/Delay' */
  SMO_Arctan_FOC_DW.Delay_DSTATE_n[SMO_Arctan_FOC_DW.CircBufIdx] =
    SMO_FOC_ELAPS_T;
  if (SMO_Arctan_FOC_DW.CircBufIdx < 2U) {
    SMO_Arctan_FOC_DW.CircBufIdx++;
  } else {
    SMO_Arctan_FOC_DW.CircBufIdx = 0U;
  }

  /* End of Update for Delay: '<S24>/Delay' */

  /* Update for UnitDelay: '<S31>/Unit Delay' */
  SMO_Arctan_FOC_DW.UnitDelay_DSTATE_c = SMO_Arctan_FOC_DW.Delay_DSTATE_a;

  /* Update for Delay: '<S61>/Delay' */
  SMO_Arctan_FOC_DW.Delay_DSTATE_ca[SMO_Arctan_FOC_DW.CircBufIdx_n] =
    SwitchCaseActionSubsystem1_ELAP;
  if (SMO_Arctan_FOC_DW.CircBufIdx_n < 2U) {
    SMO_Arctan_FOC_DW.CircBufIdx_n++;
  } else {
    SMO_Arctan_FOC_DW.CircBufIdx_n = 0U;
  }

  /* End of Update for Delay: '<S61>/Delay' */

  /* Update for UnitDelay: '<S68>/Unit Delay' */
  SMO_Arctan_FOC_DW.UnitDelay_DSTATE_l = SMO_Arctan_FOC_DW.Delay_DSTATE_gm;
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
